close all;
clear;

%% 1. 加载拟合数据
dat_path = 'datas\resamp.dat';
[vgcp_x, vgcp_y, vgcp_z, vgcp_col, vgcp_row, img_size] = loadResampDat(dat_path);

%% 2. 拟合模型: 物方到像方
fprintf("RPC fitting backward ...\n");
rpc_model_backward = hsRPC.fitting(vgcp_x, vgcp_y, vgcp_z, vgcp_col, vgcp_row ...
                    , 'backward' ...
                    , "ImgSize", img_size);
fprintf("\n");

fprintf("RPC fitting forward ...\n");
rpc_model_forward = hsRPC.fitting(vgcp_x, vgcp_y, vgcp_z, vgcp_col, vgcp_row ...
                    , 'forward' ...
                    , "ImgSize", img_size);
fprintf("\n");

%% 3. 进行投影
[est_col, est_row] = hsRPC.project(rpc_model_backward, vgcp_x, vgcp_y, vgcp_z);
error_col = est_col - vgcp_col;
error_row = est_row - vgcp_row;

figure;
subplot(211);
plot(error_col);
title("backward error col");
grid on;
subplot(212);
plot(error_row);
title("backward error row");
grid on;

[est_lon, est_lat] = hsRPC.project(rpc_model_forward, vgcp_col, vgcp_row, vgcp_z);
error_x = est_lon - vgcp_x;
error_y = est_lat - vgcp_y;

figure;
subplot(211);
plot(error_x);
title("forward error x");
grid on;
subplot(212);
plot(error_y);
title("forward error y");
grid on;

%% 4. 保存/读取模型
rpc_backward_path = 'datas\fitting_backward.rpc';
hsRPC.write(rpc_model_backward, rpc_backward_path);
rpc_model_backward_2 = hsRPC.read(rpc_backward_path);

rpc_forward_path = 'datas\fitting_forward.rpc';
hsRPC.write(rpc_model_forward, rpc_forward_path);
rpc_model_forward_2 = hsRPC.read(rpc_forward_path);

%% 5. 计算验证集的误差
verify_path = 'datas\resamp_verify.dat';
[verify_x, verify_y, verify_z, verify_col, verify_row, ~] = loadResampDat(verify_path);

[est_col_2, est_row_2] = hsRPC.project(rpc_model_backward_2, verify_x, verify_y, verify_z);
error_col_2 = est_col_2 - verify_col;
error_row_2 = est_row_2 - verify_row;

figure;
subplot(211);
plot(error_col_2);
title("verify backward error col");
grid on;
subplot(212);
plot(error_row_2);
title("verify backward error row");
grid on;

[est_lon_2, est_lat_2] = hsRPC.project(rpc_model_forward_2, verify_col, verify_row, verify_z);
error_x_2 = est_lon_2 - verify_x;
error_y_2 = est_lat_2 - verify_y;

figure;
subplot(211);
plot(error_x_2);
title("verify forward error x");
grid on;
subplot(212);
plot(error_y_2);
title("verify forward error y");
grid on;

